/**
  ******************************************************************************
  * @file           : communicate_task.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/2/15
  ******************************************************************************
  */



#include "communicate_task.h"
#include "iostream"


extern osSemaphoreId_t HurtDataUpdateBinarySemHandle;
extern osSemaphoreId_t HPDataUpdateBinarySemHandle;

extern rc_device_t g_rc;
extern float yaw_gyro;
extern receive_judge_t judge_rece_mesg;
extern uint8_t robot_data_update;
extern uint8_t is_flip_over;
extern Super_Cap_t superCap;
extern SuperCapState super_cap_state;
extern uint8_t is_on_slope;
extern uint8_t is_use_capacitor;
extern custom_info_t sentry_to_engineer;
extern custom_info_t sentry_to_hero;

chassis_info_tran chassis_info;
radar_speed speed_radar={};
judge_info_0 judgeInfo0={};
judge_info_1 judgeInfo1={};
judge_info_2 judgeInfo2={};
judge_info_3 judgeInfo3={};
judge_HP_info_0 judgeHPInfo0={1,1,1,1}; //初始值防止上电为0检测为复活
judge_HP_info_1 judgeHPInfo1={1,1,1,1}; //初始值防止上电为0检测为复活
judge_HP_info_2 judgeHPInfo2={1,1,0}; //初始值防止上电为0检测为复活
chassis_yaw_info chassis_yaw_cal;
float chassis_real_yaw = 0;
sentry_decision_info sentryDecisionMake;
super_cap_tran superCapTran;
enemy_coo_tran enemyCooTran;
enemy_coo enemyCoo;
sentry_to_robot sentryToRobot;
std::string string_tmp;
aerial_operator_data aerialOperatorData;

void can_rc_callback(can_device_receive *can_receive, uint8_t *data){
    auto rc_tran = (rc_info_tran *)data;
    rc_update(&g_rc, data);
    g_rc.is_lost = rc_tran->is_lost;
}

void can_speed_callback(can_device_receive *can_receive, uint8_t *data){
    auto speed_tran = (speed_info *)data;
    speed_radar.speed_x = speed_tran->speed_x;
    speed_radar.speed_y = speed_tran->speed_y;
    speed_radar.angle = atan2(speed_radar.speed_y, speed_radar.speed_x);
    speed_radar.angle = (speed_radar.angle < 0 ? speed_radar.angle + 2 * PI : speed_radar.angle) / 2.f / PI * 360.f;
}


void can_chassis_yaw_cal_callback(can_device_receive *can_receive, uint8_t *data){
    chassis_yaw_info_tran *chassis_yaw_cal_tran = (chassis_yaw_info_tran *)data;

    chassis_yaw_cal.chassis_yaw = chassis_yaw_cal_tran->chassis_yaw;
    chassis_yaw_cal.is_target_detected = chassis_yaw_cal_tran->is_target_detected;
    chassis_yaw_cal.is_chassis_yaw_stop = chassis_yaw_cal_tran->is_chassis_yaw_stop;
    chassis_yaw_cal.is_spin = chassis_yaw_cal_tran->is_spin;
    chassis_yaw_cal.is_use_capacitor = chassis_yaw_cal_tran->is_use_capacitor;
    chassis_yaw_cal.high_speed_spin = chassis_yaw_cal_tran->high_speed_spin;
    chassis_yaw_cal.high_speed_translation = chassis_yaw_cal_tran->high_speed_translation;
    chassis_yaw_cal.slope_speed_translation = chassis_yaw_cal_tran->slope_speed_translation;

    is_use_capacitor = (is_on_slope == 1 || chassis_yaw_cal.is_use_capacitor == 1) ? 1 : 0;
}

uint32_t sentry_decision_cnt = 0;
void can_chassis_yaw_real_callback(can_device_receive *can_receive, uint8_t *data){
    yaw_and_decision_info *yaw_and_decision_info_t = (yaw_and_decision_info *)data;

    chassis_real_yaw = yaw_and_decision_info_t->chassis_yaw_real;
    sentry_decision_cnt ++;
    sentryDecisionMake.exchange_ammo_amount = yaw_and_decision_info_t->exchange_ammo_amount;
    sentryDecisionMake.is_confirm_revive = yaw_and_decision_info_t->is_confirm_revive;
    sentryDecisionMake.is_exchange_immediate_revive = yaw_and_decision_info_t->is_exchange_immediate_revive;
    sentryDecisionMake.remote_exchange_ammo_request_count = yaw_and_decision_info_t->remote_exchange_ammo_request_count;
    sentryDecisionMake.remote_exchange_health_request_count = yaw_and_decision_info_t->remote_exchange_health_request_count;
    if(sentry_decision_cnt > 30){     //接收频率为1000HZ，这里需限频
        sentry_decision_cnt = 0;
        sentry_cmd_judge_update();
    }
}

/*
 * 超级电容发回数据解包
 */
void can_super_capacitor_callback(can_device_receive *can_receive, uint8_t *data) {
    float V_SC;
    superCap.power_fb.p_sc = (int16_t)(data[0] | (data[1] << 8)) / 50.f;           //0.02W/LSB
    superCap.power_fb.p_wheel = (int16_t)(data[2] | (data[3] << 8)) / 50.f;        //0.02W/LSB
    V_SC = (int16_t)(data[4] | (data[5] << 8)) / 1000.f;                           //mV -> V
    superCap.scFeedback.V_SC = superCap.scFeedback.V_SC * 0.9f + V_SC * 0.1f;      //低通
    super_cap_state = (SuperCapState)data[6];
}

uint8_t interaction_judge_send_cnt = 0;
void can_sentry_interaction_callback(can_device_receive *can_receive, uint8_t *data){
    sentry_to_robot_tran *sentry_to_robot_tran_t = (sentry_to_robot_tran *)data;

    sentryToRobot.sentry_state = sentry_to_robot_tran_t->sentry_state;
    sentryToRobot.sentry_event = sentry_to_robot_tran_t->sentry_event;
    sentryToRobot.sentry_destination = sentry_to_robot_tran_t->sentry_destination;

    if(sentryToRobot.sentry_destination == healing_point){
        string_tmp = "sentry go home\n\n";
    }else{
        string_tmp = "sentry normal\n\n";
    }
    utf8_TO_utf16((uint8_t *)string_tmp.c_str(), sizeof(string_tmp),(uint16_t *)sentry_to_engineer.user_data, sizeof(sentry_to_engineer.user_data));

    if(sentryToRobot.sentry_destination == our_R3_patrol_zone){
        string_tmp = "sentry go R3\n\n";
    }else{
        string_tmp = "sentry normal\n\n";
    }
    utf8_TO_utf16((uint8_t *)string_tmp.c_str(), sizeof(string_tmp),(uint16_t *)sentry_to_hero.user_data, sizeof(sentry_to_hero.user_data));

    switch(interaction_judge_send_cnt){
        case 0:
            sentry_to_aerial_update();      //接收频率为20HZ，这里不用限频
            break;
        case 1:
            sentry_draw_ui_to_engineer();
            break;
        case 2:
            sentry_draw_ui_to_hero();
            break;
    }
    interaction_judge_send_cnt++;
    interaction_judge_send_cnt = interaction_judge_send_cnt > 2 ? 0 : interaction_judge_send_cnt;
}

//与雷达站的通信
void can_sentry_to_radar_callback(can_device_receive *can_receive, uint8_t *data){
    enemy_coo_tran *enemy_coo_tran_t = (enemy_coo_tran *)data;

    enemyCoo.robot_number = enemy_coo_tran_t->robot_number;
    enemyCoo.is_reliable = enemy_coo_tran_t->is_reliable;
    enemyCoo.x = (float)enemy_coo_tran_t->x / 100.0f;
    enemyCoo.y = (float)enemy_coo_tran_t->y / 100.0f;
    sentry_to_radar_update();      //接收频率为20HZ，这里不用限频
}

void communicate_task(void *argument){
    can_device_receive can_rc_info(&hcan2, 0x300, can_rc_callback);
    can_device_transmit can_chassis_info(&hcan2, 0x301, 8, (uint8_t *)&chassis_info);
    can_device_receive can_speed_info(&hcan2, 0x302, can_speed_callback);
    can_device_receive can_chassis_yaw_cal(&hcan2, 0x305, can_chassis_yaw_cal_callback);
    can_device_receive can_chassis_yaw_real(&hcan2, 0x308, can_chassis_yaw_real_callback);
    can_device_receive can_super_capacitor_receive(&hcan1, 0x311, can_super_capacitor_callback);
    can_device_transmit can_super_capacitor_transmit(&hcan1, 0x312, 8, (uint8_t *)&superCapTran);

    for(;;){
        taskENTER_CRITICAL();
        chassis_info.yaw_angle = yaw_gyro;
        chassis_info.is_flip_over = is_flip_over;

        taskEXIT_CRITICAL();
        can_chassis_info.can_send_msg();


        // 限幅
        superCap.scSet.set_power_charge = superCap.scSet.set_power_charge > POWER_CHARGE_MAX ? POWER_CHARGE_MAX : superCap.scSet.set_power_charge;
        superCap.scSet.set_power_charge = superCap.scSet.set_power_charge < -POWER_DISCHARGE_MAX ? -POWER_DISCHARGE_MAX : superCap.scSet.set_power_charge;
        // 电容功率单位换算 W -> mW -> 0.1mW
        int32_t power_charge = (int32_t)(superCap.scSet.set_power_charge * 1000 * 10);   // 0.1 mW/LSB

        taskENTER_CRITICAL();
        // 电容总开关 1开 0关
        superCapTran.enable = 1;
        superCapTran.power_charge = power_charge;
        superCapTran.ramp = 0;    //飞坡电容标志位，哨兵不需要，直接赋0
        superCapTran.reverse = 0;
        taskEXIT_CRITICAL();
        can_super_capacitor_transmit.can_send_msg();

        osDelay(1);
    }
}

uint8_t be_hit_cnt = 5;
uint8_t mains_power_shooter_output = 0;
uint8_t is_occupy_healing_spot = 0;
void communicate_judge_task(void *argument){
    uint8_t rfid_tmp = 0x01;
    can_device_transmit can_judge_info_0(&hcan2, 0x303, 8, (uint8_t *)&judgeInfo0);
    can_device_transmit can_judge_info_1(&hcan2, 0x304, 8, (uint8_t *)&judgeInfo1);
    can_device_transmit can_judge_info_2(&hcan2, 0x30a, 8, (uint8_t *)&judgeInfo2);
    can_device_transmit can_judge_info_3(&hcan2, 0x30f, 8, (uint8_t *)&judgeInfo3);
    osSemaphoreAcquire(HurtDataUpdateBinarySemHandle, 0);
    for(;;){
        taskENTER_CRITICAL();
        judgeInfo0.current_HP = judge_rece_mesg.robot_status_data.current_HP;
        judgeInfo0.shooter_barrel_cooling_value = judge_rece_mesg.robot_status_data.shooter_barrel_cooling_value;
        judgeInfo0.game_progress = judge_rece_mesg.game_status_data.game_progress;
        judgeInfo0.stage_remain_time = judge_rece_mesg.game_status_data.stage_remain_time;
        judgeInfo0.hurt_armor_id = judge_rece_mesg.hurt_data.armor_id;
        judgeInfo0.HP_deduction_reason = (uint8_t)judge_rece_mesg.hurt_data.HP_deduction_reason;
        mains_power_shooter_output = judge_rece_mesg.robot_status_data.mains_power_shooter_output;
        judgeInfo0.mains_power_shooter_output = mains_power_shooter_output;
        is_occupy_healing_spot = (uint8_t)(judge_rece_mesg.rfid_status_data.rfid_status >> 13 & rfid_tmp);
        judgeInfo0.is_occupy_patrol_zone = (uint8_t)(judge_rece_mesg.rfid_status_data.rfid_status >> 14 & rfid_tmp);
        judgeInfo0.is_occupy_healing_spot = is_occupy_healing_spot;
        osStatus_t Status = osSemaphoreAcquire(HurtDataUpdateBinarySemHandle, 0);

        be_hit_cnt = Status == osOK ? 0 : be_hit_cnt;
        if(be_hit_cnt < 10){
            judgeInfo0.be_hit = 1;
            be_hit_cnt++;
        }else{
            judgeInfo0.be_hit = 0;
        }

        taskEXIT_CRITICAL();
        can_judge_info_0.can_send_msg();
        osDelay(5);

        if(robot_data_update == 1){
        taskENTER_CRITICAL();
        judgeInfo1.is_blue = judge_rece_mesg.robot_status_data.robot_id > 100 ? 1 : 0;
        judgeInfo1.maximum_HP = judge_rece_mesg.robot_status_data.maximum_HP;
        judgeInfo1.shooter_barrel_heat_limit = judge_rece_mesg.robot_status_data.shooter_barrel_heat_limit;
        judgeInfo1.game_type = judge_rece_mesg.game_status_data.game_type;
        judgeInfo1.remaining_gold_coin = judge_rece_mesg.projectile_allowance_data.remaining_gold_coin;

        taskEXIT_CRITICAL();
            can_judge_info_1.can_send_msg();
        }
        osDelay(5);

        taskENTER_CRITICAL();
        judgeInfo2.projectile_allowance_17mm = judge_rece_mesg.projectile_allowance_data.projectile_allowance_17mm;
        judgeInfo2.shooter_id = judge_rece_mesg.shoot_data.shooter_id;
        judgeInfo2.bullet_speed = judge_rece_mesg.shoot_data.bullet_speed;

        taskEXIT_CRITICAL();
        can_judge_info_2.can_send_msg();
        osDelay(5);

        taskENTER_CRITICAL();
        judgeInfo3.shooter_17mm_1_barrel_heat = judge_rece_mesg.power_heat_data.shooter_17mm_1_barrel_heat;
        judgeInfo3.shooter_17mm_2_barrel_heat = judge_rece_mesg.power_heat_data.shooter_17mm_2_barrel_heat;

        taskEXIT_CRITICAL();
        can_judge_info_3.can_send_msg();
        osDelay(5);
    }
}

void communicate_HP_task(void *argument){
    can_device_transmit can_HP_info_0(&hcan2, 0x306, 8, (uint8_t *)&judgeHPInfo0);
    can_device_transmit can_HP_info_1(&hcan2, 0x307, 8, (uint8_t *)&judgeHPInfo1);
    can_device_transmit can_HP_info_2(&hcan2, 0x309, 8, (uint8_t *)&judgeHPInfo2);
    osSemaphoreAcquire(HPDataUpdateBinarySemHandle, 0);
    for(;;) {
        osSemaphoreAcquire(HPDataUpdateBinarySemHandle, osWaitForever);
        if (judge_rece_mesg.robot_status_data.robot_id > 100) { //自身蓝方，敌方红方
            judgeHPInfo0.enemy_1_robot_HP = judge_rece_mesg.game_robot_HP_data.red_1_robot_HP;
            judgeHPInfo0.enemy_2_robot_HP = judge_rece_mesg.game_robot_HP_data.red_2_robot_HP;
            judgeHPInfo0.enemy_3_robot_HP = judge_rece_mesg.game_robot_HP_data.red_3_robot_HP;
            judgeHPInfo0.enemy_4_robot_HP = judge_rece_mesg.game_robot_HP_data.red_4_robot_HP;
            judgeHPInfo1.enemy_5_robot_HP = judge_rece_mesg.game_robot_HP_data.red_5_robot_HP;
            judgeHPInfo1.enemy_7_robot_HP = judge_rece_mesg.game_robot_HP_data.red_7_robot_HP;
            judgeHPInfo1.enemy_outpost_HP = judge_rece_mesg.game_robot_HP_data.red_outpost_HP;
            judgeHPInfo1.enemy_base_HP = judge_rece_mesg.game_robot_HP_data.red_base_HP;
            judgeHPInfo2.our_outpost_HP = judge_rece_mesg.game_robot_HP_data.blue_outpost_HP;
            judgeHPInfo2.our_base_HP = judge_rece_mesg.game_robot_HP_data.blue_base_HP;
        }else{ //自身红方，敌方蓝方
            judgeHPInfo0.enemy_1_robot_HP = judge_rece_mesg.game_robot_HP_data.blue_1_robot_HP;
            judgeHPInfo0.enemy_2_robot_HP = judge_rece_mesg.game_robot_HP_data.blue_2_robot_HP;
            judgeHPInfo0.enemy_3_robot_HP = judge_rece_mesg.game_robot_HP_data.blue_3_robot_HP;
            judgeHPInfo0.enemy_4_robot_HP = judge_rece_mesg.game_robot_HP_data.blue_4_robot_HP;
            judgeHPInfo1.enemy_5_robot_HP = judge_rece_mesg.game_robot_HP_data.blue_5_robot_HP;
            judgeHPInfo1.enemy_7_robot_HP = judge_rece_mesg.game_robot_HP_data.blue_7_robot_HP;
            judgeHPInfo1.enemy_outpost_HP = judge_rece_mesg.game_robot_HP_data.blue_outpost_HP;
            judgeHPInfo1.enemy_base_HP = judge_rece_mesg.game_robot_HP_data.blue_base_HP;
            judgeHPInfo2.our_outpost_HP = judge_rece_mesg.game_robot_HP_data.red_outpost_HP;
            judgeHPInfo2.our_base_HP = judge_rece_mesg.game_robot_HP_data.red_base_HP;
        }
        //因为是发进队列所以不必担心冲突
        can_HP_info_0.can_send_msg();
        can_HP_info_1.can_send_msg();
        can_HP_info_2.can_send_msg();
    }
}

void communicate_robot_interaction_task(void *argument){
    can_device_transmit can_radar_info(&hcan2, 0x30b, 8, (uint8_t *)&enemyCooTran);
    can_device_receive can_sentry_interaction_info(&hcan2, 0x30c, can_sentry_interaction_callback);
    can_device_receive can_sentry_to_radar(&hcan2, 0x30d, can_sentry_to_radar_callback);
    can_device_transmit can_operator_info(&hcan2, 0x30e, 8, (uint8_t *)&aerialOperatorData);
    for(;;){
        enemyCooTran.robot_number = hero;
        enemyCooTran.is_reliable = judge_rece_mesg.radar_data.hero.is_reliable;
        enemyCooTran.x = (uint16_t)((judge_rece_mesg.radar_data.hero.x > 28 ? 28 : judge_rece_mesg.radar_data.hero.x) * 100);
        enemyCooTran.y = (uint16_t)((judge_rece_mesg.radar_data.hero.y > 28 ? 28 : judge_rece_mesg.radar_data.hero.y) * 100);
        can_radar_info.can_send_msg();
        osDelay(100);

        enemyCooTran.robot_number = engineer;
        enemyCooTran.is_reliable = judge_rece_mesg.radar_data.engineer.is_reliable;
        enemyCooTran.x = (uint16_t)((judge_rece_mesg.radar_data.engineer.x > 28 ? 28 : judge_rece_mesg.radar_data.engineer.x) * 100);
        enemyCooTran.y = (uint16_t)((judge_rece_mesg.radar_data.engineer.y > 28 ? 28 : judge_rece_mesg.radar_data.engineer.y) * 100);
        can_radar_info.can_send_msg();
        osDelay(100);

        enemyCooTran.robot_number = infantry_3;
        enemyCooTran.is_reliable = judge_rece_mesg.radar_data.infantry_3.is_reliable;
        enemyCooTran.x = (uint16_t)((judge_rece_mesg.radar_data.infantry_3.x > 28 ? 28 : judge_rece_mesg.radar_data.infantry_3.x) * 100);
        enemyCooTran.y = (uint16_t)((judge_rece_mesg.radar_data.infantry_3.y > 28 ? 28 : judge_rece_mesg.radar_data.infantry_3.y) * 100);
        can_radar_info.can_send_msg();
        osDelay(100);

        enemyCooTran.robot_number = infantry_4;
        enemyCooTran.is_reliable = judge_rece_mesg.radar_data.infantry_4.is_reliable;
        enemyCooTran.x = (uint16_t)((judge_rece_mesg.radar_data.infantry_4.x > 28 ? 28 : judge_rece_mesg.radar_data.infantry_4.x) * 100);
        enemyCooTran.y = (uint16_t)((judge_rece_mesg.radar_data.infantry_4.y > 28 ? 28 : judge_rece_mesg.radar_data.infantry_4.y) * 100);
        can_radar_info.can_send_msg();
        osDelay(100);

        enemyCooTran.robot_number = infantry_5;
        enemyCooTran.is_reliable = judge_rece_mesg.radar_data.infantry_5.is_reliable;
        enemyCooTran.x = (uint16_t)((judge_rece_mesg.radar_data.infantry_5.x > 28 ? 28 : judge_rece_mesg.radar_data.infantry_5.x) * 100);
        enemyCooTran.y = (uint16_t)((judge_rece_mesg.radar_data.infantry_5.y > 28 ? 28 : judge_rece_mesg.radar_data.infantry_5.y) * 100);
        can_radar_info.can_send_msg();
        osDelay(100);

        enemyCooTran.robot_number = sentry;
        enemyCooTran.is_reliable = judge_rece_mesg.radar_data.sentry.is_reliable;
        enemyCooTran.x = (uint16_t)((judge_rece_mesg.radar_data.sentry.x > 28 ? 28 : judge_rece_mesg.radar_data.sentry.x) * 100);
        enemyCooTran.y = (uint16_t)((judge_rece_mesg.radar_data.sentry.y > 28 ? 28 : judge_rece_mesg.radar_data.sentry.y) * 100);
        can_radar_info.can_send_msg();
        osDelay(100);

        aerialOperatorData.cmd_keyboard = judge_rece_mesg.map_command.cmd_keyboard;
        aerialOperatorData.position_x = (uint16_t)(judge_rece_mesg.map_command.target_position_x * 100);
        aerialOperatorData.position_y = (uint16_t)(judge_rece_mesg.map_command.target_position_y * 100);
        can_operator_info.can_send_msg();
        osDelay(100);
    }
}



/* 接收到的数据包 */
extern unpack_data_t     judge_unpack_obj;
/* 裁判系统数据保护结构体 */
extern judgement_protection_struct judgement_protection;
extern osEventFlagsId_t refereeEventHandle;

/**
  * @brief  解包任务.
  * @param  无
  * @retval 无
  */
void judge_unpack_task(void *argument) {
    uint32_t event;
    /* open judge uart receive it */
    judgement_uart_init();
    while (1) {
        //当接收到数据时事件set,开启数据解包处理
        event = osEventFlagsWait(refereeEventHandle,
                                 UART_IDLE_SIGNAL,
                                 osFlagsWaitAny,
                                 500);
        //当超时时（0.5s）尝试重启整个串口
        if(event == osErrorTimeout) {
            judgement_protection.judgement_lost_flag = 1; //裁判系统丢失
            HAL_UART_MspDeInit(&huart6);
            HAL_UART_MspInit(&huart6);
            judgement_uart_init();
        }
            //串口空闲中断完成，FIFO正常收到数据，开始解包
        else if(event & UART_IDLE_SIGNAL) {
            unpack_fifo_data(&judge_unpack_obj, DN_REG_ID);
        }
            //对除超时外的其他错误情况进行保护，重启串口
        else{
            judgement_protection.judgement_lost_flag = 1; //裁判系统丢失
            HAL_UART_MspDeInit(&huart6);
            HAL_UART_MspInit(&huart6);
            judgement_uart_init();
        }
    }
}


/**
  * @brief  选手端与机器人交互
  * @param  无
  * @retval 无
  */
void StulnteractiveTask(void *argument)
{
    /* USER CODE BEGIN StulnteractiveTask */
    /* Infinite loop */
    uint32_t event;
    for(;;)
    {
        event = osEventFlagsWait(refereeEventHandle,
                                 UART_TX_SIGNAL,
                                 osFlagsWaitAny,
                                 osWaitForever);
        if (event & UART_TX_SIGNAL)
        {
            //开始发送数据
            send_packed_fifo_data(DN_REG_ID);
        }
    }
    /* USER CODE END StulnteractiveTask */
}


